#include <EEPROM.h>

static uint8_t checkNewConf = 145;

typedef struct eep_entry_t{
  void *  var;
  uint8_t size;
};

// ************************************************************************************************************
// EEPROM Layout definition
// ************************************************************************************************************
static eep_entry_t eep_entry[] = {
  &checkNewConf, sizeof(checkNewConf),
  &P8, sizeof(P8)
, &I8, sizeof(I8) 
, &DD8, sizeof(DD8) 
, &rcRate8, sizeof(rcRate8)
, &rcExpo8, sizeof(rcExpo8)
, &rollPitchRate, sizeof(rollPitchRate)
, &yawRate, sizeof(yawRate)
, &dynThrPID, sizeof(dynThrPID)
, &activate, sizeof(activate)
, &accZero, sizeof(accZero)
, &magZero, sizeof(magZero)
, &accTrim, sizeof(accTrim)
#if defined(POWERMETER)
, &powerTrigger1, sizeof(powerTrigger1)
#endif
};  
#define EEBLOCK_SIZE sizeof(eep_entry)/sizeof(eep_entry_t)
// ************************************************************************************************************
unsigned char  EEPLoadData(unsigned int address,unsigned char *ptr,unsigned char length)
{
  uint8 i;
  uint16 data;
  for(i=0;i<length;i++)
  {
    EEPROM.read(address+i,&data);
    ptr[i]=(uint8)data;
  }
}
unsigned char  EEPSaveData(unsigned int address,unsigned char *ptr,unsigned char length)
{
  uint8 i;
  uint16 DataWrite ;
  for(i=0;i<length;i++)
  EEPROM.write(address+i,ptr[i]);
}
 	

void readEEPROM() {
  uint8_t i, _address = eep_entry[0].size;\
  
  for(i=1; i<EEBLOCK_SIZE; i++) {
    EEPLoadData( (_address), (uint8 *)eep_entry[i].var,eep_entry[i].size);_address += eep_entry[i].size;
    //eeprom_read_block(eep_entry[i].var, (void*)(_address), eep_entry[i].size); _address += eep_entry[i].size;
  }  
  #if defined(POWERMETER)
    pAlarm = (uint32_t) powerTrigger1 * (uint32_t) PLEVELSCALE * (uint32_t) PLEVELDIV; // need to cast before multiplying
  #endif
  for(i=0;i<7;i++) lookupRX[i] = (2500+rcExpo8*(i*i-25))*i*(int32_t)rcRate8/1250;
}

void writeParams() {
  uint8_t i, _address = 0;
  for(i=0; i<EEBLOCK_SIZE; i++) {
    EEPSaveData( (_address), (uint8 *)eep_entry[i].var,eep_entry[i].size);_address += eep_entry[i].size;
    //eeprom_write_block(eep_entry[i].var, (void*)(_address), eep_entry[i].size); _address += eep_entry[i].size;
  }  
  readEEPROM();
  blinkLED(15,20,1);
}


void SetParamDefault(void)
{
   P8[ROLL] = 40; I8[ROLL] = 30; DD8[ROLL] = 17;
  P8[PITCH] = 40; I8[PITCH] = 30; DD8[PITCH] = 17;
  P8[YAW]  = 85; I8[YAW]  = 0;  DD8[YAW]  = 0;
  P8[PIDALT]  = 47; I8[PIDALT]  = 0;  DD8[PIDALT]  = 0;
  P8[PIDVEL]  =  0; I8[PIDVEL]  = 0;  DD8[PIDVEL]  = 0;
  P8[PIDLEVEL] = 45; I8[PIDLEVEL] = 45;
  P8[PIDMAG] = 40;
  rcRate8 = 45; // = 0.9 in GUI
  rcExpo8 = 65;
  rollPitchRate = 0;
  yawRate = 0;
  dynThrPID = 0;
  for(uint8_t i=0;i<6;i++) activate[i] = 0;
  accTrim[0] = 0; accTrim[1] = 0;
#if defined(POWERMETER)
  powerTrigger1 = 0;
#endif
  writeParams();
}
void checkFirstTime() {
  uint8_t test_val; 
  EEPLoadData(0,&test_val ,sizeof(test_val));
  //eeprom_read_block(, (void*)(0), sizeof(test_val));
  if (test_val == checkNewConf) return;
  SetParamDefault();
  
  
}
